﻿/**
 * Trabalho elaborado por Alexandre Leitão, aluno nº 10331
 * e
 * Pedro Lemos Figueiredo, aluno nº 10326
 */
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Collections;

namespace eda1314_10331_10326
{
    class PathFinder
    {
        private List<double> timePoint;
        /// <summary>
        /// Algoritmo A*
        /// </summary>
        /// <param name="start">Ponto inicial</param>
        /// <param name="end">Ponto final</param>
        /// <param name="image">Contém os valores de x e y de cada ponto</param>
        /// <param name="img">Contém a imagem utilizada pelo algoritmo</param>
        /// <returns>Lista de pontos que são o caminho entre o ponto inicial e final</returns>
        public List<Point> Algorithm(Point start, Point end, int[,] image, Bitmap img)
        {
            List<Point> closedList = new List<Point>();
            List<Point> openList = new List<Point>();
            List<Point> path = new List<Point>();

                 this.timePoint = new List<double>();

            ArrayList successors = new ArrayList();
            start.setGCost(0);
            start.setFCost(distance_between(start, end) + start.getGCost()); 
            openList.Add(start);

            while (openList.Count > 0)
            {
                /*
                    System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
                    watch.Start();
                */
                var list = (from p in openList
                            orderby p.getFCost() ascending
                            select p);
                Point currentPoint = list.First();
                /*
                    watch.Stop();
                    this.timePoint.Add(watch.ElapsedMilliseconds);
                */
                if (currentPoint.Equals(end))
                {
                    path.Add(currentPoint);
                    img.SetPixel(currentPoint.getPointX(), currentPoint.getPointY(), Color.Red);
                    this.construct_path(path, currentPoint, img);
                    break;
                }
                openList.Remove(currentPoint);
                closedList.Add(currentPoint);
                this.getSuccessors(image, successors, currentPoint, end);
                foreach (Point PointSucessor in successors)
                {
                    if(closedList.Contains(PointSucessor))
                    {
                        continue;
                    }
                    double tentative_cost = currentPoint.getGCost() + (PointSucessor.getGCost() - currentPoint.getGCost());
                    if (openList.Contains(PointSucessor) == false || tentative_cost < PointSucessor.getGCost())
                    {
                        int index = path.IndexOf(PointSucessor);
                        PointSucessor.setGCost(tentative_cost);
                        PointSucessor.setFCost(PointSucessor.getGCost() + distance_between(PointSucessor, end));
                        if (openList.Contains(PointSucessor) == false)
                        {
                            openList.Add(PointSucessor);
                        }
                    }   
                }
                closedList.Add(currentPoint);
            }
            return path;
        }
        /// <summary>
        /// Devolve a distância de um ponto até outro
        /// </summary>
        /// <param name="A">Primeiro ponto</param>
        /// <param name="B">O outro ponto</param>
        /// <returns>Distância</returns>
        private double distance_between(Point A, Point B)
        {
            int xsqrtDistance = (A.getPointX() - B.getPointX()) * (A.getPointX() - B.getPointX());
            int ysqrtDistance = (A.getPointY() - B.getPointY()) * (A.getPointY() - B.getPointY());
            double hipotenusa = (int)Math.Sqrt(Math.Abs(xsqrtDistance) + Math.Abs(ysqrtDistance));
            return hipotenusa;
        }
        /// <summary>
        /// Desenha o caminho na imagem e guarda o caminho na lista de caminho principal (path)
        /// </summary>
        /// <param name="path">Lista de pontos que representam o caminho</param>
        /// <param name="current">Ponto</param>
        /// <param name="img">Imagem</param>
        private void construct_path(List<Point> path, Point current, Bitmap img)
        {
            while (current.getParent() != null)
            {
                img.SetPixel(current.getPointX(), current.getPointY(), Color.Red);
                path.Add(current.getParent());
                current = current.getParent();
            }

        }
        /// <summary>
        /// Descobre os pontos adjacentes com as cores pretendidas
        /// </summary>
        /// <param name="image">Array com as posições x e y dos pixéis</param>
        /// <param name="successors">Lista de arrays que será esvaziada e repreenchida com novos valores</param>
        /// <param name="father">Ponto a que serão procurados os adjacentes</param>
        /// <param name="end">Ponto</param>
        private void getSuccessors(int[,] image, ArrayList successors, Point father, Point end)
        {
            successors.Clear();
            for (int x = -1; x <= 1; x++)
            {
                for (int y = -1; y <= 1; y++ )
                {
                    if ((x == 0 && y == 0) || ((((double)image[father.getPointX() + 
                                                x, father.getPointY() + y]) / 255) < 0.16))
                    {
                        
                    }
                    else
                    {
                        Point brat = new Point(father.getPointX() + x, father.getPointY() + y);
                        brat.setParent(father);
                        brat.setGCost(father.getGCost() + distance_between(father, brat));
                        brat.setFCost(brat.getGCost() + distance_between(brat, end));
                        successors.Add(brat);
                    }
                }
            }
        }
        /// <summary>
        /// Altera a variável timePoint
        /// </summary>
        /// <returns>Devolve uma lista de doubles</returns>
        public List<double> getTimeList()
        {
            return this.timePoint;
        }
    }

    class Point
    {
        private int[] coor = new int[2];
        private Point parent;
        private double gCost;
        private double cost;
        private double hipotenusa;
        /// <summary>
        /// Construtor da classe ponto
        /// </summary>
        /// <param name="x">Coordenada x</param>
        /// <param name="y">Coordenada y</param>
        public Point(int x, int y)
        {
            this.coor[0] = x;
            this.coor[1] = y;
        }
        /// <summary>
        /// Getter das coordenadas
        /// </summary>
        /// <returns>array de ints com valores das coordenadas</returns>
        public int[] getPoint()
        {
            return this.coor;
        }
        /// <summary>
        /// Cria uma relação entre pontos
        /// </summary>
        /// <param name="father">Ponto</param>
        public void setParent(Point father)
        {
            this.parent = father;
        }
        /// <summary>
        /// getter de relação parent
        /// </summary>
        /// <returns>ponto parent</returns>
        public Point getParent()
        {
            return this.parent;
        }
        /// <summary>
        /// Altera o GCost
        /// </summary>
        /// <param name="cost">double do tipo cost</param>
        public void setGCost(double cost) 
        {
            this.gCost = cost;
        }
        /// <summary>
        /// getter do custo g
        /// </summary>
        /// <returns>custo G</returns>
        public double getGCost()
        {
            return this.gCost;
        }
        /// <summary>
        /// Getter do custo total
        /// </summary>
        /// <returns>Devolve o custo total</returns>
        public double getFCost() 
        {
            return this.cost;
        }
        /// <summary>
        /// Getter do ponto x do ponto
        /// </summary>
        /// <returns>coordenada x</returns>
        public int getPointX()
        {
            return this.coor[0];
        }
        /// <summary>
        /// Getter do ponto y do ponto
        /// </summary>
        /// <returns>coordenada y</returns>
        public int getPointY()
        {
            return this.coor[1];
        }
        /// <summary>
        /// Altera o custo total
        /// </summary>
        /// <param name="cost">custo</param>
        public void setFCost(double cost)
        {
            this.cost = cost;
        }
        /// <summary>
        /// Substitui o equals de object para não comparar endereços de memória
        /// </summary>
        /// <param name="obj">objeto</param>
        /// <returns>devolve confirmação se dois pontos são iguais (caso os objetos não sejam pontos</returns>
        public override bool Equals(object obj)
        {
            if (obj == null)
            {
                return false;
            }
            else
            {
                Point p = (Point)obj;
                return ((this.getPointX() == p.getPointX()) && 
                        (this.getPointY() == p.getPointY()));
            }
        }
        /// <summary>
        /// Substitui método to string
        /// </summary>
        /// <returns>posição do ponto e custo</returns>
        public override string ToString()
        {
            string point = "X = " + this.getPointX() + ";Y = " + this.getPointY() + ";Cost = "  + this.getFCost() + ";";
            return point;
        }
        /// <summary>
        /// Getter dos pontos anteriores
        /// </summary>
        /// <returns>arraylist com os pais</returns>
        public ArrayList getParentList()
        {
            ArrayList l = new ArrayList();
            Point p = this;
            do
            {
                l.Add(p);
            } while ((p = p.getParent()) != null);
            return l;
        }
    }
}